#include <string.h>
#include <sys/param.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h" 
#include "freertos/event_groups.h"
#include "esp_system.h"
#include "esp_wifi.h"
#include "esp_event.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "tcpip_adapter.h"
#include "protocol_examples_common.h"

#include "lwip/err.h"
#include "lwip/sockets.h"
#include "lwip/sys.h"
#include <lwip/netdb.h>
#include "m_ble.h"
#include "m_nvs.h"

#include <time.h>
#include "driver/uart.h"
#include "driver/gpio.h"

#include "m_http_client.h"
#include "m_led.h"
#include "m_uart.h"
#include "m_config.h"


#ifdef usr_use_position_or_heartbreath
#define TimeRTC         1*1000*1000
#define TimePeriod      3*1000*1000
#define TimeTimeout     20*1000*1000
#else
#define TimeRTC         500*1000
#define TimePeriod      500*1000
#define TimeTimeout     20*1000*1000
#endif

static const char *TAG = "M_UDP_CLIENT";
static const char *payload = "Message from ESP32 ";

extern ble_esp32_param_t m_ble_esp32_param;


extern uint8_t isWifiConnectOk;
uint8_t isUdpConnectOk = 0;
static uint8_t socket_Reply_Cnt = 0;
static uint8_t socket_Retry_Connect_Cnt = 0;

extern xQueueHandle led_evt_queue;
extern led_status_e led_status;
xQueueHandle app_data_queue = NULL; //app 数据队列


static int sock=0;

static uint8_t isOnePackage = 0;
static uint8_t devbuf[2048];
static uint16_t devbufLen = 0;
static char devstr[2048];
static uint16_t devstrLen = 0;
position_config_t position;
position_config_t position_Old;

static uint8_t devmac[6] = {0};
static char devmacBuf[12+1] = {0};

struct sockaddr_in dest_addr;


void udp_connect_process(void);
void socket_reconnect_process(void)
{
    socket_Reply_Cnt = 0;
    isUdpConnectOk = 0;
    close(sock);
    udp_connect_process();
}

static uint8_t isPositionRP = 1;
static uint8_t isPositionHeart = 0;
esp_timer_handle_t Positioning_timer_handle = 0;
void app_Positioning_time_timer_cb(void)
{
    isPositionHeart = 1;
    
    // 如果网络断掉了
    if(isUdpConnectOk == 0)
    {
       esp_timer_stop(Positioning_timer_handle);
       socket_Reply_Cnt = 0;
    }
    else    // 如果检测到服务器，超过20次发送还没有回复，则重启
    {
        if(socket_Reply_Cnt++ == 100)
        {
            socket_reconnect_process();
        }
    }
}

esp_timer_handle_t Timeout_timer_handle = 0;
void app_Timeout_time_timer_cb(void)
{
    esp_timer_stop(Positioning_timer_handle);
    esp_timer_start_periodic(Positioning_timer_handle, TimePeriod); 
    isPositionRP = 1;
}

void udp_client_rcv_task(void *pvParameters)
{
    int len = 0;
    char databuf[1024];
    while(1)
    {
        if(isUdpConnectOk)
        {
            vTaskDelay(2500 / portTICK_PERIOD_MS);
            len = recv(sock, databuf, 128, 0);
            if (len > 0) 
            {
                ESP_LOGI(TAG, "rcvDataLen:%d", len);
                socket_Reply_Cnt = 0;

                if(len==21)
                {
                    if(databuf[20] == 'R') // 实时
                    {
                        esp_timer_stop(Positioning_timer_handle); 
                        esp_timer_start_periodic(Positioning_timer_handle, TimeRTC); 
                        esp_timer_start_once(Timeout_timer_handle, TimeTimeout); 
                        isPositionRP = 0;
                    }
                    else if(databuf[20] == 'P')   // 周期
                    {
                        esp_timer_stop(Positioning_timer_handle);
                        esp_timer_start_periodic(Positioning_timer_handle, TimePeriod); 
                        esp_timer_stop(Timeout_timer_handle);
                        isPositionRP = 1;
                    }
                }
            }
            else
            {
                break;
            }
        }
        else
        {
            vTaskDelay(100 / portTICK_PERIOD_MS);
        }
    }
    vTaskDelete(NULL);
}


void udp_client_task(void *pvParameters)
{
    // 队列
    uint8_t buf[256];
    app_data_queue = xQueueCreate(20, 256);

    while (1) {

        if (xQueueReceive(app_data_queue, buf, portMAX_DELAY))
        {
            #ifdef usr_use_position_or_heartbreath
            if(isUdpConnectOk)
            {
                // 如果收到房间面积信息，则启动周期心跳定时器（此时认为毫米波开始工作了）
                if ((buf[1] == 0xA1) && (buf[2] == 0xA1) && (buf[buf[0]] == 0xB1) && (buf[buf[0]-1] == 0xB1))
                {

                }

                //ESP_LOGE("","app_data_queue len:%d,%s\r\n",buf[0],buf+1);
                if ((buf[1] == 0xA3) && (buf[2] == 0xA3) && (buf[buf[0]] == 0xB3) && (buf[buf[0]-1] == 0xB3))
                {
                    esp_err_t err;
                    err = esp_timer_start_periodic(Positioning_timer_handle, TimePeriod); 
                    if(err == ESP_OK)
                    {
                        printf("Positioning time timer  start ok!\r\n");
                    }	

                    memcpy(devbuf,buf+1,buf[0]);
                    devbufLen = buf[0];

                    isOnePackage = 2;
                    //ESP_LOGE("","A3B3\r\n");
                }


                if ((buf[1] == 0xA3) && (buf[2] == 0xA3) && (buf[buf[0]] != 0xB3) && (buf[buf[0]-1] != 0xB3))
                {
                    memcpy(devbuf,buf+1,buf[0]);
                    devbufLen = buf[0];

                    isOnePackage = 1;
                    //ESP_LOGE("","A3\r\n");
                }

                if((buf[1] != 0xA3) && (buf[2] != 0xA3) && (buf[buf[0]] == 0xB3) && (buf[buf[0]-1] == 0xB3))
                {
                    memcpy(devbuf+devbufLen,buf+1,buf[0]);
                    devbufLen += buf[0];

                    isOnePackage = 2;
                    ESP_LOGE("","B3\r\n");
                }

                memset(buf,0,256);

                if(isOnePackage == 2)
                {
                    devstrLen = sprintf(devstr,"FT#");
                    for(int k=0;k<devbufLen;k++)
                    {
                        sprintf(devstr+devstrLen+k*2,"%02X",devbuf[k]);
                    }
                    devstrLen += devbufLen*2;
                    //printf("devstrLen111: %d\r\n",devstrLen);

                    if(isPositionRP)
                    {
                        devstrLen += sprintf(devstr+devstrLen,"#\r\n%s*\r\nReport=P$\r\n",devmacBuf);
                    }
                    else
                    {
                        devstrLen += sprintf(devstr+devstrLen,"#\r\n%s*\r\nReport=R$\r\n",devmacBuf);
                    }
                    //printf("devstrLen222: %d\r\n",devstrLen);

                    // UDP的ERROR
                    int udperr = 0;
                    //isPositionHeart = 1;  // 测试实时数据
                    if(!isPositionHeart)
                    {
                        // 轨迹数uint16 由于最多16 所以只取低8位即可
                        position.cnt = devbuf[4];
                        //printf("posture.cnt: %d\r\n",position.cnt);
                        for(int i=0;i<position.cnt;i++)
                        {
                            uint16_t id =  devbuf[6+i*12] + (devbuf[6+i*12]<<8);
                            position.param[i].channel = id & 0x000F; 
                            position.param[i].posture = (id>>4) & 0x0003; 
                            //printf("i: %d,channel: %d,posture: %d\r\n",i,position.param[i].channel,position.param[i].posture);
                        }

                        // 首先判断检测的cnt
                        if(position_Old.cnt != position.cnt)
                        {
                            udperr = sendto(sock, devstr, devstrLen, 0, (struct sockaddr *)&dest_addr, sizeof(dest_addr));
                            ESP_LOGE("","position.cnt Change\r\n");
                        }
                        else
                        {
                            for(int j=0;j<position.cnt;j++)
                            {
                                if((position_Old.param[j].channel != position.param[j].channel) || (position_Old.param[j].posture != position.param[j].posture))
                                {
                                    udperr = sendto(sock, devstr, devstrLen, 0, (struct sockaddr *)&dest_addr, sizeof(dest_addr));
                                    ESP_LOGE("","position.param Change\r\n");
                                    break;
                                }
                            }
                        }
                    }
                    else
                    {
                        isPositionHeart = 0;
                        udperr = sendto(sock, devstr, devstrLen, 0, (struct sockaddr *)&dest_addr, sizeof(dest_addr));
                        ESP_LOGE("","position Heart\r\n");
                    }

                    // 将当前数据赋给old数据
                    position_Old = position;
                    //position.cnt = 0;

                    // 如果socket发送失败，则判定socket连接断开
                    if (udperr < 0) {
                    ESP_LOGE(TAG, "Error occurred during sending: errno %d", errno);
                    socket_reconnect_process();
                    }

                    memset(devbuf,0,256);
                    devbufLen = 0; 
                    memset(devstr,0,1024);
                    devstrLen = 0;

                    isOnePackage = 0;
                }
            }
            #else
            if(isUdpConnectOk)
            {
                int udperr = 0;
                // 如果收到毫米波呼吸心率数据
                if ((buf[1] == 0xA4) && (buf[2] == 0xA4) && (buf[buf[0]] == 0xB4) && (buf[buf[0]-1] == 0xB4))
                {
                    esp_err_t err;
                    err = esp_timer_start_periodic(Positioning_timer_handle, TimePeriod); 
                    if(err == ESP_OK)
                    {
                        printf("Positioning time timer  start ok!\r\n");
                    }

                    memcpy(devbuf,buf+1,buf[0]);
                    devbufLen = buf[0];
                    isOnePackage = 2;
                }

                memset(buf,0,256);

                if(isOnePackage == 2)
                {
                    devstrLen = sprintf(devstr,"FT#");
                    for(int k=0;k<devbufLen;k++)
                    {
                        sprintf(devstr+devstrLen+k*2,"%02X",devbuf[k]);
                    }
                    devstrLen += devbufLen*2;

                    devstrLen += sprintf(devstr+devstrLen,"#\r\n%s*\r\nReport=P$\r\n",devmacBuf);

                    // UDP的ERROR
                    if(isPositionHeart)
                    {
                        isPositionHeart = 0;
                        int udperr = 0;
                        udperr = sendto(sock, devstr, devstrLen, 0, (struct sockaddr *)&dest_addr, sizeof(dest_addr));
                        ESP_LOGE("","Heart rate breathing\r\n");
                    }

                    // 如果socket发送失败，则判定socket连接断开
                    if (udperr < 0) {
                    ESP_LOGE(TAG, "Error occurred during sending: errno %d", errno);
                    socket_reconnect_process();
                    }

                    memset(devbuf,0,256);
                    devbufLen = 0; 
                    memset(devstr,0,1024);
                    devstrLen = 0;

                    isOnePackage = 0;
                }
            }
            #endif
        }
    }
}


void socket_init(void)
{
#ifdef usr_use_wifi_or_eth
    esp_read_mac(devmac, ESP_MAC_WIFI_STA);//读取WIFI MAC地址
#else
    esp_read_mac(devmac, ESP_MAC_ETH);//读取ETH MAC地址
#endif
    sprintf((char*)devmacBuf,"%02X%02X%02X%02X%02X%02X",devmac[0],devmac[1],devmac[2],devmac[3],devmac[4],devmac[5]);
    printf("MAC:%s",devmacBuf);

    // 定时器
    esp_err_t err;
	esp_timer_create_args_t app_Positioning_time_timer = 
	{ 
		.callback = &app_Positioning_time_timer_cb, 							//回调函数
		.arg = NULL, 													        //参数
		.name = "Positioning time timer" 										//定时器名称
	};

    // 创建定时器时
	err = esp_timer_create(&app_Positioning_time_timer, &Positioning_timer_handle);
    if(err == ESP_OK)
    {
        printf("Positioning time timer cteate ok!\r\n");
    }

    // 创建一个超时定时器，防止下发周期P的指令没有收到，强制复位为周期Report=P
	esp_timer_create_args_t app_Timeout_time_timer = 
	{ 
		.callback = &app_Timeout_time_timer_cb, 							//回调函数
		.arg = NULL, 													    //参数
		.name = "Timeout time timer" 										//定时器名称
	};	
    esp_timer_create(&app_Timeout_time_timer, &Timeout_timer_handle);

    xTaskCreate(udp_client_task, "udp_client", 4096, NULL, 6, NULL);
}

void udp_connect_process(void)
{
    // 等待200ms，刷新一下led闪烁延时while流程
    vTaskDelay(200 / portTICK_RATE_MS);
    led_status = led_status_socket_conneting;
    xQueueSend(led_evt_queue, &led_status, NULL);

    //延时3S准备建立client，也起到让LED跑一会的作用
    vTaskDelay(4000 / portTICK_RATE_MS);


	char tcp_server_address[16];
	char tcp_server_port[6];
	uint16_t tcp_port = 0;
    int addr_family;
    int ip_protocol;

    HexArrayToString(m_ble_esp32_param.ble_char3_IP,m_ble_esp32_param.ble_char3_IP_len,tcp_server_address);
    HexArrayToString(m_ble_esp32_param.ble_char4_Port,m_ble_esp32_param.ble_char4_Port_len,tcp_server_port);
    tcp_port = strtoul(tcp_server_port,0,10);

    dest_addr.sin_addr.s_addr = inet_addr(tcp_server_address);
    //dest_addr.sin_addr.s_addr = inet_addr("192.168.7.75");
    dest_addr.sin_family = AF_INET;
    dest_addr.sin_port = htons(tcp_port);
    addr_family = AF_INET;
    ip_protocol = IPPROTO_IP;
    //inet_ntoa_r(dest_addr.sin_addr, addr_str, sizeof(addr_str) - 1);

    sock = socket(addr_family, SOCK_DGRAM, ip_protocol);
    if (sock < 0) {
        ESP_LOGE(TAG, "Unable to create socket: errno %d", errno);
        isUdpConnectOk = 0;

        // UDP重连20次不行，重启设备
        if(socket_Retry_Connect_Cnt++ == 20)
        {
            socket_Retry_Connect_Cnt = 0;
            esp_restart();
        }

        socket_reconnect_process();
    }
    else
    {
        ESP_LOGW(TAG, "Socket created, sending to  %s  :  %d", tcp_server_address, tcp_port);

        socket_Retry_Connect_Cnt = 0;

        led_status = led_status_socket_connected;
        xQueueSend(led_evt_queue, &led_status, NULL);   

        isUdpConnectOk = 1;
        xTaskCreate(udp_client_rcv_task, "udp_client_rcv", 4096, NULL, 7, NULL);
    }
}

void m_udp_send(uint8_t *data,uint32_t length)
{
    int udperr=0;
    udperr=sendto(sock, data, length, 0, (struct sockaddr *)&dest_addr, sizeof(dest_addr));
    // 如果socket发送失败，则判定socket连接断开
    if (udperr < 0) {
    ESP_LOGE(TAG, "Error occurred during sending: errno %d", errno);
    }
    else
    {
        ESP_LOGE(TAG, "OKOKOKOKOKOKOKOKOK");
    }
}




